#ifndef H_MIXER
#define H_MIXER

#include "Arduino.h"

class Mixer
{
private:
  bool armed;

public:
  float rcRate,rcExpo;


  void setup()
  {
    armed=false;
    rcRate=1.2;
    rcExpo=2.5;
  }

  void load()
  {
    rcRate=para.readFloat();
    rcExpo=para.readFloat();
  }

  void save()
  {
    para.writeFloat(rcRate);
    para.writeFloat(rcExpo);
  }

  void update()
  {
    static int countArm;
    if (armed && rx.channelIn[RUD] < CHLEFT && rx.channelIn[THR] < CHDOWN) // disarm
    {
      armed=false;
      esc.setState(0);
      led.setOutput(0);
      pRoll.reset();
      pPitch.reset();
      pYaw.reset();
    }
    else if (!armed)
    {
      if (rx.channelIn[RUD] > CHRIGHT && rx.channelIn[THR] < CHDOWN) // arm it
      {
        if (countArm++ >= 1*CYCLERATE) // 1 second
        {
          countArm=0;
          armed=true;
          esc.setState(1);
          led.setOutput(BLINKARMED);
        }
      }
#ifdef GYRO_PRESENT
      else if (rx.channelIn[RUD] < CHLEFT && rx.channelIn[THR] < CHDOWN && rx.channelIn[ELE] < CHDOWN) // TODO check also ail
      {
        gyro.setState(CALIBRATE); // search GYRO zero
        led.setOutput(BLINKFAST);
      }
#endif
#ifdef ACC_PRESENT
      else if (rx.channelIn[RUD] < CHLEFT && rx.channelIn[THR] > CHUP && rx.channelIn[ELE] < CHDOWN) // TODO check also ail
      {
        acc.setState(CALIBRATE); // search ACC zero
        led.setOutput(BLINKFAST);
      }
#endif
#ifdef MAG_PRESENT
      else if (rx.channelIn[RUD] < CHLEFT && rx.channelIn[THR] < CHDOWN && rx.channelIn[ELE] > CHUP) // TODO check also ail
      {
        mag.setState(CALIBRATE); // calibrate MAG
        led.setOutput(BLINKFAST);
      }
#endif
    }

    float ail,ele,thr,rud;

    thr=rx.channelIn[THR];

    if (armed)
    {
      if (thr<-0.7)
      {
        thr=-0.7;
        for(byte i=0;i<MAXCHANOUT;i++)
          esc.channelOut[i]=thr;

      }
      else
      {
        if (rx.channelIn[AUX]>-0.7) // stable mode
        {
          ail=  pRoll.update(rx.channelIn[AIL]*rcRate-ahrs.r*rcExpo, gyro.r);
          ele= pPitch.update(rx.channelIn[ELE]*rcRate-ahrs.p*rcExpo, gyro.p);
        }
        else
        {
          ail=  pRoll.update(rx.channelIn[AIL]*rcRate, gyro.r);
          ele= pPitch.update(rx.channelIn[ELE]*rcRate, gyro.p);
        }
        rud=   pYaw.update(rx.channelIn[RUD]*2*rcRate,-gyro.y); // it is better to flight *2 NOTE that the right hand rule give you the reverse of the RUD channelIn

#if defined(QUADX)
        esc.channelOut[FRONTL]=thr-ele*ISIN45/2+ail*ISIN45/2-rud;
        esc.channelOut[FRONTR]=thr-ele*ISIN45/2-ail*ISIN45/2+rud;
        esc.channelOut[REARL ]=thr+ele*ISIN45/2+ail*ISIN45/2+rud;
        esc.channelOut[REARR ]=thr+ele*ISIN45/2-ail*ISIN45/2-rud;
#elif defined(HEXAX)
        esc.channelOut[FRONTL]=thr-ele*ISIN60/2+ail/2-rud; // ISIN60=1/sin(60 deg)=0.866
        esc.channelOut[FRONTR]=thr-ele*ISIN60/2-ail/2+rud;
        esc.channelOut[REARL ]=thr+ele*ISIN60/2+ail/2-rud;
        esc.channelOut[REARR ]=thr+ele*ISIN60/2-ail/2+rud;
        esc.channelOut[LEFT  ]=thr             +ail/2+rud;
        esc.channelOut[RIGHT ]=thr             -ail/2-rud;  
#elif defined(HEXAP)
        esc.channelOut[FRONT ]=thr-ele/2  +rud;
        esc.channelOut[REAR  ]=thr+ele/2  -rud;
        esc.channelOut[FRONTL]=thr-ele/2+ail*ISIN60/2-rud; // sin(60 deg)=0.866
        esc.channelOut[FRONTR]=thr-ele/2-ail*ISIN60/2-rud;
        esc.channelOut[REARL ]=thr+ele/2+ail*ISIN60/2+rud;
        esc.channelOut[REARR ]=thr+ele/2-ail*ISIN60/2+rud;
#endif


      }
    }
    else
    {
      for(byte i=0;i<MAXCHANOUT;i++)
        esc.channelOut[i]=-1;
    }

#if defined(CAMSTAB)  // check if you can
    esc.channelOut[CAMROLL ]=limitf(CAMMINR, CAMMIDR+CAMMULR*ahrs.r, CAMMAXR);
    esc.channelOut[CAMPITCH]=limitf(CAMMINP, CAMMIDP+CAMMULP*ahrs.p, CAMMAXP);
#endif
  }

} 
mixer;


#endif






















